#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <signal.h>

int fd;

int open_tty()
{
    struct termios termio;
    int r;

    memset (&termio, 0, sizeof (termio));

    if ((fd = open ("/dev/ttyUSB1", O_RDWR | O_NDELAY | O_NOCTTY)) == -1) {
        exit(-1);
    }

    fcntl(fd, F_SETFL, 0);
    r = tcgetattr(fd, &termio);

    /*
     *set the baud rate to 38400.
     */
    cfsetspeed(&termio, B38400);

    /*
     *set parity to none.
     */
    termio.c_cflag &= ~(PARENB | PARODD);

    /*
     *set databits to 8.
     */
    termio.c_cflag = ((termio.c_cflag & ~CSIZE) | CS8);

    /*
     *set flow control to none.
     */
    termio.c_cflag &= ~(CRTSCTS);
    termio.c_iflag &= ~(IXON | IXOFF | IXANY);
    termio.c_cflag |= (CLOCAL | CREAD);

    /*
     *apply our device config.
     */
    r = tcsetattr(fd, TCSANOW, &termio);

    if (r == -1) {
        exit(-1);
    }

    return fd;
}

void sig_handler(int sig_num)
{
    close(fd);
    printf("Exiting...\n");
    exit(0);
}

int main(int argc, char** argv)
{   
    unsigned char cmd[7];
    unsigned char resp;
    unsigned char r[7];
    int count, checksum, n, i;
    struct termios options;

    signal(SIGINT, &sig_handler);

    memset(cmd, 0, sizeof(cmd));

    if(argc < 3 || argc > 16){
        printf("Usage: %s [address] [command] [data] ...\n", argv[0]);
        return -1;
    }

    fd = open_tty();

    int fcount = 0;

    printf("\n");
    for(int i = 0; i < 1400; i++) {
        //printf("index = [%d]\n", i);

        checksum = 0;
        cmd[0] = 58;
        for(int j = 1; j < argc; j++){
            cmd[j] = atoi(argv[j]);
            checksum += cmd[j];
        }
        checksum -= cmd[5];

        cmd[5] = rand() % 100;
        checksum += cmd[5];

        checksum = (256 - (58 + checksum)) % 256;
        cmd[argc] = checksum;
        //printf("sending ...\n");

        //printf("\ncmd = ");
        for(int j = 0; j <= argc; j++){
            //printf("%d ", cmd[j]);
        }

        count = write(fd, cmd, sizeof(cmd));
        //printf("[%d:%d:%d:%d:%d:%d:%d] WRITE\n", cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], cmd[6]);

        //setbuf(stdout, NULL);
        //printf("READING...\n");
        unsigned int count_b = 0;
        unsigned int n = 0, tmp = 0;

        int nfds = 0;
        fd_set readfds;
        FD_ZERO(&readfds);
        FD_SET(fd, &readfds);

        struct timeval *pto;
        struct timeval timeout;

        pto = &timeout;
        timeout.tv_sec = 0;
        timeout.tv_usec = 20000;

        if(1 != select(fd+1, &readfds, NULL, NULL, pto)){
            fprintf(stderr, "FAILED count = %d, index = %d\n", ++fcount, i);
            continue;
        }

        if(!FD_ISSET(fd, &readfds)){
            continue;
        }
         
        int k = 0;
        while(read(fd, &resp, sizeof(resp)) != -1){
            r[k++] = resp;
            //fprintf(stderr, "resp[%d] = %d\n", n, resp);
            if(n == 2){
                tmp = resp;
            } else if(n > 2 && count_b == tmp + 2){
                break;
            }   
            n++;
            count_b++;
        }
        //printf("[%d:%d:%d:%d:%d:%d:%d] READ\n", r[0], r[1], r[2], r[3], r[4], r[5], r[6]);
    }

    close(fd);
    return 0;
}
